#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
RK3588客户端 - 整合所有功能
"""
import subprocess
import socket
import json
import time
import threading
import re
import struct
from datetime import datetime

try:
    import serial
except ImportError:
    serial = None

try:
    from pymodbus.client import ModbusSerialClient
except ImportError:
    try:
        from pymodbus.client.sync import ModbusSerialClient
    except ImportError:
        ModbusSerialClient = None

class PLCController:
    """PLC控制器"""
    def __init__(self, port='/dev/ttyUSB0'):
        self.port = port
        self.client = None
        self.connected = False
        
    def connect(self):
        if not ModbusSerialClient:
            return False
        try:
            self.client = ModbusSerialClient(
                port=self.port, baudrate=9600, bytesize=8,
                parity='N', stopbits=1, timeout=2
            )
            if self.client.connect():
                self.connected = True
                print(f"[PLC] 连接成功: {self.port}")
                return True
        except:
            pass
        return False
    
    def read(self, addr):
        if not self.connected:
            return None
        try:
            result = self.client.read_holding_registers(address=addr, count=1, slave=1)
            if hasattr(result, 'registers'):
                v = result.registers[0]
                return v - 65536 if v > 32767 else v
        except:
            pass
        return None
    
    def write(self, addr, value):
        if not self.connected:
            return False
        try:
            if value < 0:
                value += 65536
            self.client.write_register(address=addr, value=value, slave=1)
            return True
        except:
            return False
    
    def read_all(self):
        """读取所有传感器"""
        return {
            'generator_voltage': self.read(129),
            'generator_current': self.read(130),
            'generator_power': self.read(131),
            'generator_temp': self.read(132),
            'engine_temp': self.read(133),
            'hot_water_temp': self.read(134),
            'cooling_temp': self.read(135),
            'box_temp': self.read(136),
            'fuel_level': self.read(137),
            'water_level': self.read(138),
            'control_authority': self.read(120),
            'relay_220v': self.read(124),
            'water_spray': self.read(125),
            'speed_percent': self.read(127)
        }

class RK3588Client:
    def __init__(self, robot_id="GZJY001", server_ip="192.168.10.1"):
        self.robot_id = robot_id
        self.server_ip = server_ip
        self.running = False
        self.cameras = []
        self.processes = []
        self.plc = PLCController()
        
    def detect_cameras(self):
        """检测摄像头"""
        try:
            result = subprocess.run(['v4l2-ctl', '--list-devices'], capture_output=True, text=True)
            for pattern, name in [("USB Camera", "前置"), ("LRCP.*F1080P", "后置")]:
                lines = result.stdout.split('\n')
                for i, line in enumerate(lines):
                    if re.search(pattern, line):
                        for j in range(i+1, min(i+5, len(lines))):
                            match = re.search(r'/dev/video(\d+)', lines[j])
                            if match:
                                self.cameras.append({
                                    "device": f"/dev/video{match.group(1)}",
                                    "port": 8080 + len(self.cameras),
                                    "name": name
                                })
                                break
                        break
            return len(self.cameras) > 0
        except:
            return False
    
    def start_camera(self, cam):
        """启动摄像头"""
        cmd = ['gst-launch-1.0', '-q', 'v4l2src', f'device={cam["device"]}',
               '!', 'image/jpeg,width=1280,height=720,framerate=30/1',
               '!', 'multipartmux', 'boundary=frame',
               '!', 'tcpserversink', 'host=0.0.0.0', f'port={cam["port"]}', 'sync=false']
        try:
            p = subprocess.Popen(cmd, stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
            time.sleep(2)
            if p.poll() is None:
                print(f"[{cam['name']}] 启动 端口:{cam['port']}")
                return p
        except:
            pass
        return None
    
    def lidar_loop(self):
        """激光雷达线程"""
        if not serial:
            return
        
        try:
            ser = serial.Serial('/dev/ttyACM0', 230400, timeout=1)
            sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
            sock.connect((self.server_ip, 9001))
            print("[雷达] 连接成功")
            
            scan_data = [None] * 360
            buffer = bytearray()
            last_send = time.time()
            
            while self.running:
                data = ser.read(1024)
                if data:
                    buffer.extend(data)
                    while len(buffer) >= 47:
                        if buffer[0] == 0x54:
                            packet = buffer[:47]
                            buffer = buffer[47:]
                            try:
                                start_angle = struct.unpack('<H', packet[4:6])[0] / 100.0
                                for i in range(12):
                                    offset = 6 + i * 3
                                    distance = struct.unpack('<H', packet[offset:offset+2])[0]
                                    intensity = packet[offset+2]
                                    if distance > 0:
                                        angle = start_angle + i * 0.75
                                        if angle >= 360:
                                            angle -= 360
                                        idx = int(angle)
                                        if 0 <= idx < 360:
                                            scan_data[idx] = {'angle': round(angle, 2), 'distance': distance, 'intensity': intensity}
                            except:
                                pass
                        else:
                            buffer = buffer[1:]
                
                if time.time() - last_send >= 1.0:
                    points = [p for p in scan_data if p]
                    sock.send((json.dumps({
                        'type': 'lidar_parsed',
                        'robot_id': self.robot_id,
                        'points': points,
                        'point_count': len(points)
                    }) + '\n').encode())
                    last_send = time.time()
            
            ser.close()
            sock.close()
        except Exception as e:
            print(f"[雷达] 错误: {e}")
    
    def gps_loop(self):
        """GPS线程"""
        if not serial:
            return
        
        try:
            ser = serial.Serial('/dev/ttyUSB1', 9600, timeout=1)
            sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
            sock.connect((self.server_ip, 9002))
            print("[GPS] 连接成功 (ttyUSB1)")
            
            gps_data = {'latitude': 0, 'longitude': 0, 'satellites': 0, 'fix_type': 'No Fix', 'altitude': 0, 'speed': 0}
            line_count = 0
            send_count = 0
            loop_count = 0
            
            print("[GPS] 开始读取循环...")
            
            while self.running:
                loop_count += 1
                if loop_count % 50 == 1:
                    print(f"[GPS] 循环{loop_count}次, in_waiting={ser.in_waiting}")
                
                if ser.in_waiting > 0:
                    line = ser.readline().decode('ascii', errors='ignore').strip()
                    line_count += 1
                    
                    # 每20行打印一次
                    if line_count % 20 == 1:
                        print(f"[GPS] 已读{line_count}行, 示例: {line[:50]}")
                    
                    if line.startswith('$GPGGA') or line.startswith('$GNGGA'):
                        parts = line.split(',')
                        if len(parts) >= 15:
                            try:
                                if parts[2] and parts[3]:
                                    lat = float(parts[2][:2]) + float(parts[2][2:]) / 60.0
                                    gps_data['latitude'] = -lat if parts[3] == 'S' else lat
                                if parts[4] and parts[5]:
                                    lon = float(parts[4][:3]) + float(parts[4][3:]) / 60.0
                                    gps_data['longitude'] = -lon if parts[5] == 'W' else lon
                                if parts[7]:
                                    gps_data['satellites'] = int(parts[7])
                                if parts[6]:
                                    gps_data['fix_type'] = {'0': 'No Fix', '1': 'GPS Fix', '2': 'DGPS Fix'}.get(parts[6], 'Unknown')
                                
                                sock.send((json.dumps({
                                    'type': 'gps_data',
                                    'robot_id': self.robot_id,
                                    'data': gps_data
                                }) + '\n').encode())
                                
                                send_count += 1
                                if send_count % 10 == 1:
                                    print(f"[GPS] 发送#{send_count}: 纬度={gps_data['latitude']:.6f} 经度={gps_data['longitude']:.6f} 卫星={gps_data['satellites']}")
                            except Exception as e:
                                print(f"[GPS] 解析错误: {e}")
                time.sleep(0.1)
            
            ser.close()
            sock.close()
        except Exception as e:
            print(f"[GPS] 错误: {e}")
    
    def plc_loop(self):
        """PLC线程"""
        if not self.plc.connect():
            print("[PLC] 连接失败")
            return
        
        try:
            sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
            sock.connect((self.server_ip, 9003))
            print("[PLC] 数据通道连接")
            
            while self.running:
                sensors = self.plc.read_all()
                sock.send((json.dumps({
                    'type': 'sensor_data',
                    'robot_id': self.robot_id,
                    'sensors': sensors
                }) + '\n').encode())
                time.sleep(2)
            
            sock.close()
        except Exception as e:
            print(f"[PLC] 错误: {e}")
    
    def control_loop(self):
        """命令接收线程"""
        while self.running:
            try:
                sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
                sock.connect((self.server_ip, 9000))
                print("[命令] 连接成功")
                
                # 立即发送一次video消息，让Windows保存socket
                sock.send((json.dumps({
                    'type': 'video',
                    'robot_id': self.robot_id,
                    'data': {'connected': True, 'cameras': len(self.processes)}
                }) + '\n').encode())
                print(f"[命令] 已发送robot_id: {self.robot_id}")
                
                buffer = ""
                last_status = time.time()
                
                while self.running:
                    # 发送状态
                    if time.time() - last_status >= 30:
                        sock.send((json.dumps({
                            'type': 'video',
                            'robot_id': self.robot_id,
                            'data': {'connected': True, 'cameras': len(self.processes)}
                        }) + '\n').encode())
                        last_status = time.time()
                    
                    # 接收命令
                    sock.settimeout(0.1)
                    try:
                        data = sock.recv(1024)
                        if data:
                            print(f"[DEBUG] 收到{len(data)}字节")
                            buffer += data.decode('utf-8')
                            while '\n' in buffer:
                                line, buffer = buffer.split('\n', 1)
                                if line.strip():
                                    print(f"[DEBUG] 解析行: {line[:50]}")
                                    try:
                                        cmd = json.loads(line)
                                        print(f"[DEBUG] JSON成功: {cmd}")
                                        self.process_cmd(cmd)
                                    except Exception as e:
                                        print(f"[命令] 处理异常: {e}")
                                        import traceback
                                        traceback.print_exc()
                    except socket.timeout:
                        pass
                    except Exception as e:
                        print(f"[命令] 接收异常: {e}")
                    
                    time.sleep(0.05)
                
                sock.close()
            except Exception as e:
                print(f"[命令] 连接失败: {e}")
                time.sleep(5)
    
    def process_cmd(self, cmd):
        """处理命令"""
        try:
            cmd_type = cmd.get('type')
            print(f"[收到] 类型={cmd_type}")
            
            if cmd_type == 'movement':
                forward = cmd.get('forward', 0)
                turn = cmd.get('turn', 0)
                print(f"[WASD] 前进={forward} 转向={turn}")
                
                # PLC写入放到独立线程，避免阻塞
                if self.plc.connected:
                    def write_plc():
                        try:
                            self.plc.write(122, forward)
                            self.plc.write(121, turn)
                        except:
                            pass
                    threading.Thread(target=write_plc, daemon=True).start()
                else:
                    print("[WASD] PLC未连接")
            
            elif cmd_type == 'control':
                ctype = cmd.get('control_type')
                value = cmd.get('value', 0)
                print(f"[控制] {ctype}={value}")
                
                if self.plc.connected:
                    def write_control():
                        try:
                            if ctype == 'control_authority':
                                self.plc.write(120, value)
                            elif ctype == 'relay_220v':
                                self.plc.write(124, value)
                            elif ctype == 'water_spray':
                                self.plc.write(125, value)
                        except:
                            pass
                    threading.Thread(target=write_control, daemon=True).start()
                    
            elif cmd_type == 'continuous_forward':
                duration = cmd.get('duration', 5000) / 1000.0
                print(f"[测试] 前进{duration}秒")
                
                if self.plc.connected:
                    def test_forward():
                        try:
                            self.plc.write(122, 15)
                            time.sleep(duration)
                            self.plc.write(122, 0)
                            print("[测试] 完成")
                        except:
                            pass
                    threading.Thread(target=test_forward, daemon=True).start()
                    
        except Exception as e:
            print(f"[命令] 处理错误: {e}")
    
    def start(self):
        """启动客户端"""
        print("=" * 60)
        print(f"  机器人: {self.robot_id}")
        print(f"  服务器: {self.server_ip}")
        print("=" * 60)
        
        # 检测摄像头
        if self.detect_cameras():
            print(f"[摄像头] 检测到 {len(self.cameras)} 个")
        
        # 清理旧进程
        subprocess.run(['pkill', '-f', 'gst-launch'], capture_output=True)
        time.sleep(1)
        
        # 启动摄像头
        for cam in self.cameras:
            subprocess.run(['chmod', '666', cam['device']], capture_output=True)
            p = self.start_camera(cam)
            if p:
                self.processes.append(p)
        
        self.running = True
        
        # 启动所有线程
        threading.Thread(target=self.control_loop, daemon=True).start()
        threading.Thread(target=self.lidar_loop, daemon=True).start()
        threading.Thread(target=self.gps_loop, daemon=True).start()
        threading.Thread(target=self.plc_loop, daemon=True).start()
        
        print("=" * 60)
        print("  运行中... Ctrl+C 停止")
        print("=" * 60)
        
        try:
            while self.running:
                time.sleep(1)
        except KeyboardInterrupt:
            print("\n[停止]")
        finally:
            self.running = False
            for p in self.processes:
                if p:
                    p.terminate()

if __name__ == "__main__":
    import sys
    robot_id = sys.argv[1] if len(sys.argv) > 1 else "GZJY001"
    server_ip = sys.argv[2] if len(sys.argv) > 2 else "192.168.10.1"
    
    client = RK3588Client(robot_id, server_ip)
    client.start()

